"""
飞机类 + 轨迹规划
"""
import math
from params import R_MIN, V_LOOP, LOITER_C, LOITER_PHI0, T_LOOP

class Aircraft:
    """
    设置每一个飞机的参数
    """
    def __init__(self, idx):
        self.idx = idx# 飞机的编号
        self.t = 0.0# 飞机当前时间
        self.phase = 0  # 0 盘旋 1 转场 2 到达
        self.path = []
        self.L = 0.0
        self.v = V_LOOP
        self.s = 0.0  # 已飞路径
        self.x, self.y = 0.0, 0.0
        self.phi = 0.0# 飞机朝向
        self.target = (0.0, 0.0)
        self.loiter_trail = []  # 记录盘旋轨迹供绘图

    # ---------------- 飞机旋转动力角度计算----------------
    def loiter_step(self):
        """圆周盘旋"""
        w = 2 * math.pi / T_LOOP                    # 角速度 T_LOOP 没多少米旋转一圈
        phi = self.t * w + LOITER_PHI0[self.idx]    # 当前圆心角
        c = LOITER_C[self.idx]                      # 盘旋中心
        # 圆上位置：半径 R_MIN，相位 phi
        self.x = c[0] + R_MIN * math.cos(phi)
        self.y = c[1] + R_MIN * math.sin(phi)
        # 机头指向切线方向（向前）
        self.phi = phi + math.pi / 2
        # 记录轨迹
        if self.t <= 5.0:
            self.loiter_trail.append((self.x, self.y))
    
    '''
    路径：  P₀────P₁────P₂────P₃
    段长：  L₀    L₁    L₂
    acc：   0     L₀   L₀+L₁  L₀+L₁+L₂
    已飞 s：        ↑
                落在 L₁ 段
    t = (s − L₀)/L₁
    '''
    def follow_path_step(self, dt):
        """沿直线path 飞，每 DT 更新一次"""
        if not self.path:
            return
        self.s += self.v * dt
        acc = 0.0#累加的距离
        for i in range(len(self.path) - 1):
            a, b = self.path[i], self.path[i + 1]
            seg_len = math.hypot(b[0] - a[0], b[1] - a[1])#这一段的长度
            if acc + seg_len >= self.s:#判断在不在这一段
                t = (self.s - acc) / seg_len#判断是在这一段的哪一段
                self.x = a[0] + t * (b[0] - a[0])
                self.y = a[1] + t * (b[1] - a[1])
                self.phi = math.atan2(b[1] - a[1], b[0] - a[0])
                return
            acc += seg_len
        # 到达终点
        self.x, self.y = self.path[-1]
        self.phase = 2

    def step(self, dt):
        """总步进接口"""
        self.t += dt
        if self.phase == 0:
            self.loiter_step()
        elif self.phase == 1:
            self.follow_path_step(dt)